﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace fhe
{
  using math;
  namespace physics
  {
    public class Spring
    {
      public PointMass P1 { get; set; }

      public PointMass P2 { get; set; }

      public double PropGain { get; set; }

      public double DiffGain { get; set; }

      public const double DefaultPropGain = 100;

      public const double DefaultDiffGain = 1;

      public Spring()
      {
        PropGain = DefaultPropGain;
        DiffGain = DefaultDiffGain;
      }

      private double? baseLength;

      public void Update(TimeSpan ts)
      {
        double dt = ts.TotalSeconds;
        if ( P1 != null && P2 != null)
        {
          if (baseLength == null)
          {
            baseLength = (P1.Position - P2.Position).Length();
          }
          Vec2 diff = P2.Position - P1.Position;
          Vec2 normDiff = diff.Normalize();
          Vec2 propForce = normDiff * ( ( diff.Length() - baseLength.Value ) / baseLength.Value * PropGain);
          Vec2 diffForce = (P1.Velocity - P2.Velocity).Project(normDiff) * -DiffGain;
          Vec2 force = propForce + diffForce;
          P1.ApplyForce(force);
          P2.ApplyForce(-force);
        }
      }
    }
  }
}
